#include "at32f403a_407_board.h"
//#define SPI_DEBUG
//#define CHECKSUM
float adc_f = 0;
uint8_t g_CAN_Leg_Data[48] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD,
							  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD,
							  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD,
							  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD,
							  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD,
							  0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD};

#define MOTOR_CYCLE 1

//校验检查
uint32_t xor_checksum(uint32_t *data, size_t len)
{
	uint32_t t = 0;
	for (int i = 0; i < len; i++)
		t = t ^ data[i];
	return t;
}

void PackSPI(void)
{
	//直接把leg_msg一一赋值给SPI_Tx缓存区
	for (int i = 0; i < 92; i++)
	{
		//赋值
		SPI_Slave_Buffer_Tx[i] = ((uint8_t *)(&leg_msg))[i];
	}
//	for (int i = 0; i < 92; i++)
//	{
//		printf("Tx[%d]:%x\r\n",i,SPI_Slave_Buffer_Tx[i]);
//	}
//	printf("leg_msg_q_abad:%f,%f\r\n", leg_msg.q_abad[0], leg_msg.q_abad[1]);
//	printf("leg_msg_q_hip:%f,%f\r\n", leg_msg.q_hip[0], leg_msg.q_hip[1]);
//	printf("leg_msg_q_knee:%f,%f\r\n", leg_msg.q_knee[0], leg_msg.q_knee[1]);
//	printf("leg_msg_qd_abad:%f,%f\r\n", leg_msg.qd_abad[0], leg_msg.qd_abad[1]);
//	printf("leg_msg_qd_hip:%f,%f\r\n", leg_msg.qd_hip[0], leg_msg.qd_hip[1]);
//	printf("leg_msg_qd_knee:%f,%f\r\n", leg_msg.qd_knee[0], leg_msg.qd_knee[1]);
//	printf("leg_msg_tau_abad_ff:%f,%f\r\n", leg_msg.tau_abad[0], leg_msg.tau_abad[1]);
//	printf("leg_msg_tau_hip_ff:%f,%f\r\n", leg_msg.tau_hip[0], leg_msg.tau_hip[1]);
//	printf("leg_msg_tau_knee_ff:%f,%f\r\n", leg_msg.tau_knee[0], leg_msg.tau_knee[1]);
//	printf("leg_msg_flags:%d,%d\r\n", leg_msg.flags[0], leg_msg.flags[1]);
//	printf("leg_msg_checksum:%d\r\n", leg_msg.checksum);
	// printf("PackSPI OK!!!\r\n");
}

// 解析从VIM3过来的SPI数据，需要先做数据校验
void UnpackSPI(void)
{
	//直接把读取到的SPI值直接一一赋值给leg_cmd
	for (int i = 0; i < CMD_LEN; i++)
	{
		//赋值
		((uint8_t *)(&leg_cmd))[i] = SPI_Slave_Buffer_Rx[i];
	}
	//测试阶段，先不使用校验码
#ifdef CHECKSUM
	uint32_t Calc_CheckLegOne = xor_checksum((uint32_t *)(&SPI_Slave_Buffer_Rx[0]), 16);
	uint32_t Calc_CheckLegTwo = xor_checksum((uint32_t *)(&SPI_Slave_Buffer_Rx[68]), 16);

	//软件校验算法验证传过来的数据是否正确
	if (leg_cmd.checksum != Calc_CheckLegOne)
	{
		// 标志位全部清零
		leg_cmd.flags[0] = 0;
		leg_cmd.flags[1] = 0;
	}
#endif

	// printf("UnpackSPI OK!!!\r\n");
}

//按电机模块说明书计算返回的信息
float CalMotorPos(uint16_t p)
{
	return (float)((p - 32768) / 32768.0 * MOTOR_CYCLE);
}
float CalMotorVel(uint16_t v)
{
	return (float)((v - 2048) / 2048.0 * 65);
}
float CalMotorTor(uint16_t t)
{
	return (float)((t - 2048) / 2048.0 * 4);
}

void UnpackCAN(uint8_t *CAN_Rx_Data, uint8_t Leg_Index, uint8_t Leg_Part)
{
	//中间的临时变量
	uint16_t buffer[6] = {0};
	uint16_t ID;
	uint16_t p = 0;
	uint16_t v = 0;
	uint16_t t = 0;

	//读取6个字节的电机返回数据
	for (uint8_t i = 0; i < 6; i++)
	{
		buffer[i] = CAN_Rx_Data[i];
	}
	//进行位操作读取数据
	ID = buffer[0]; //这个可有可无
	p = (buffer[1] << 8) + buffer[2];
	v = (buffer[3] << 4) + (buffer[4] >> 4);
	t = ((buffer[4] & 0x000F) << 8) + buffer[5];
	//调试查看
//	printf("p:%x\r\n", p);
//	printf("v:%x\r\n", v);
//	printf("t:%x\r\n", t);
//计算电机数据
#ifndef SPI_DEBUG
	float Position = CalMotorPos(p);
	float Velocity = CalMotorVel(v);
	float Torque = CalMotorTor(t);
#else
	float Position = p;
	float Velocity = v;
	float Torque = t;
#endif
	//根据哪个腿(Leg_Index)和腿的哪个部分(Leg_Part)赋值给leg结构体相应地方
	if (Leg_Index == Leg_One)
	{
		switch (Leg_Part)
		{
		case abad:
			leg_msg.q_abad[0] = Position;
			leg_msg.qd_abad[0] = Velocity;
			leg_msg.tau_abad[0] = Torque;
			break;
		case hip:
			leg_msg.q_hip[0] = Position;
			leg_msg.qd_hip[0] = Velocity;
			leg_msg.tau_hip[0] = Torque;
			break;
		case knee:
			leg_msg.q_knee[0] = Position;
			leg_msg.qd_knee[0] = Velocity;
			leg_msg.tau_knee[0] = Torque;
			break;
		default:
			break;
		}
	}
	if (Leg_Index == Leg_Two)
	{
		switch (Leg_Part)
		{
		case abad:
			leg_msg.q_abad[1] = Position;
			leg_msg.qd_abad[1] = Velocity;
			leg_msg.tau_abad[1] = Torque;
			break;
		case hip:
			leg_msg.q_hip[1] = Position;
			leg_msg.qd_hip[1] = Velocity;
			leg_msg.tau_hip[1] = Torque;
			break;
		case knee:
			leg_msg.q_knee[1] = Position;
			leg_msg.qd_knee[1] = Velocity;
			leg_msg.tau_knee[1] = Torque;
			break;
		default:
			break;
		}
	}
}

/// 16 bit position, between -4*pi and 4*pi
/// 12 bit velocity, between -30 and + 30 rad/s
/// 12 bit current, between -4 and 4 A;
//这个CAN打包是每次一个电机,一个电机有5个参数
void PackCAN(float q_des, float v_des, float kp, float kd, float t_ff, uint8_t Flag)
{
	uint8_t buffer[8];

	/// 限幅做保护 ///
	q_des = Constraint(q_des, P_MIN, P_MAX);
	v_des = Constraint(v_des, V_MIN, V_MAX);
	kp = Constraint(kp, KP_MIN, KP_MAX);
	kd = Constraint(kd, KD_MIN, KD_MAX);
	t_ff = Constraint(t_ff, T_MIN, T_MAX);
	//浮点变成整型 ///
	uint16_t q_int = float_to_uint(q_des, P_MIN, P_MAX, 65536);
	uint16_t v_int = float_to_uint(v_des, V_MIN, V_MAX, 4096);
	uint16_t kp_int = float_to_uint(kp, KP_MIN, KP_MAX, 4096);
	uint16_t kd_int = float_to_uint(kd, KD_MIN, KD_MAX, 4096);
	uint16_t t_int = float_to_uint(t_ff, T_MIN, T_MAX, 4096);
	/// 整型数据打包相应的CAN格式 ///
	buffer[0] = q_int >> 8;
	buffer[1] = q_int & 0xFF;
	buffer[2] = v_int >> 4;
	buffer[3] = ((v_int & 0xF) << 4) | (kp_int >> 8);
	buffer[4] = kp_int & 0xFF;
	buffer[5] = kd_int >> 4;
	buffer[6] = ((kd_int & 0xF) << 4) | (t_int >> 8);
	buffer[7] = t_int & 0xff;

	//赋值给全局变量
	switch (Flag)
	{
	case Leg_One_Abad:
		for (uint8_t i = 0; i < 8; i++)
		{
			g_CAN_Leg_Data[i] = buffer[i];
		}
		break;
	case Leg_One_Hip:
		for (uint8_t i = 0; i < 8; i++)
		{
			g_CAN_Leg_Data[i + 8] = buffer[i];
		}
		break;
	case Leg_One_Knee:
		for (uint8_t i = 0; i < 8; i++)
		{
			g_CAN_Leg_Data[i + 16] = buffer[i];
		}
		break;
	case Leg_Two_Abad:
		for (uint8_t i = 0; i < 8; i++)
		{
			g_CAN_Leg_Data[i + 24] = buffer[i];
		}
		break;
	case Leg_Two_Hip:
		for (uint8_t i = 0; i < 8; i++)
		{
			g_CAN_Leg_Data[i + 32] = buffer[i];
		}
		break;
	case Leg_Two_Knee:
		for (uint8_t i = 0; i < 8; i++)
		{
			g_CAN_Leg_Data[i + 40] = buffer[i];
		}
		break;
	default:
		break;
	}
}

// CAN打包电机命令
void PackCANCmd(uint8_t Flag_Motor, uint8_t Flag)
{
	uint8_t buffer[8] = {0};
	switch (Flag_Motor)
	{
	case Flag_Motor_Start:
		/// 整型数据打包相应的CAN格式 ///
		buffer[0] = 0xFF;
		buffer[1] = 0xFF;
		buffer[2] = 0xFF;
		buffer[3] = 0xFF;
		buffer[4] = 0xFF;
		buffer[5] = 0xFF;
		buffer[6] = 0xFF;
		buffer[7] = 0xFC;
		break;
	case Flag_Motor_Stop:
		/// 整型数据打包相应的CAN格式 ///
		buffer[0] = 0xFF;
		buffer[1] = 0xFF;
		buffer[2] = 0xFF;
		buffer[3] = 0xFF;
		buffer[4] = 0xFF;
		buffer[5] = 0xFF;
		buffer[6] = 0xFF;
		buffer[7] = 0xFD;
		break;
	case Flag_Motor_SetZero:
		/// 整型数据打包相应的CAN格式 ///
		buffer[0] = 0xFF;
		buffer[1] = 0xFF;
		buffer[2] = 0xFF;
		buffer[3] = 0xFF;
		buffer[4] = 0xFF;
		buffer[5] = 0xFF;
		buffer[6] = 0xFF;
		buffer[7] = 0xFE;
		break;
		case Flag_Motor_Mode_Position:
		/// 整型数据打包相应的CAN格式 ///
		buffer[0] = 0xFF;
		buffer[1] = 0xFF;
		buffer[2] = 0xFF;
		buffer[3] = 0xFF;
		buffer[4] = 0xFF;
		buffer[5] = 0xFF;
		buffer[6] = 0xFF;
		buffer[7] = 0xFB;
		break;
			case Flag_Motor_Mode_Velocity:
		/// 整型数据打包相应的CAN格式 ///
		buffer[0] = 0xFF;
		buffer[1] = 0xFF;
		buffer[2] = 0xFF;
		buffer[3] = 0xFF;
		buffer[4] = 0xFF;
		buffer[5] = 0xFF;
		buffer[6] = 0xFF;
		buffer[7] = 0xFA;
		break;
				case Flag_Motor_Mode_Torque:
		/// 整型数据打包相应的CAN格式 ///
		buffer[0] = 0xFF;
		buffer[1] = 0xFF;
		buffer[2] = 0xFF;
		buffer[3] = 0xFF;
		buffer[4] = 0xFF;
		buffer[5] = 0xFF;
		buffer[6] = 0xFF;
		buffer[7] = 0xF9;
		break;
	default:
		//默认停止电机
		buffer[0] = 0xFF;
		buffer[1] = 0xFF;
		buffer[2] = 0xFF;
		buffer[3] = 0xFF;
		buffer[4] = 0xFF;
		buffer[5] = 0xFF;
		buffer[6] = 0xFF;
		buffer[7] = 0xFD;
		break;
	}

	//赋值给全局变量
	switch (Flag)
	{
	case Leg_One_Abad:
		for (uint8_t i = 0; i < 8; i++)
		{
			g_CAN_Leg_Data[i] = buffer[i];
		}
		break;
	case Leg_One_Hip:
		for (uint8_t i = 0; i < 8; i++)
		{
			g_CAN_Leg_Data[i + 8] = buffer[i];
		}
		break;
	case Leg_One_Knee:
		for (uint8_t i = 0; i < 8; i++)
		{
			g_CAN_Leg_Data[i + 16] = buffer[i];
		}
		break;
	case Leg_Two_Abad:
		for (uint8_t i = 0; i < 8; i++)
		{
			g_CAN_Leg_Data[i + 24] = buffer[i];
		}
		break;
	case Leg_Two_Hip:
		for (uint8_t i = 0; i < 8; i++)
		{
			g_CAN_Leg_Data[i + 32] = buffer[i];
		}
		break;
	case Leg_Two_Knee:
		for (uint8_t i = 0; i < 8; i++)
		{
			g_CAN_Leg_Data[i + 40] = buffer[i];
		}
		break;
	default:
		break;
	}
}

//两条腿，6个电机,这里根据标志位来判断电机的命令
void PackCAN_ALL(void)
{
	// 根据标志位判断电机命令
	// 3个电机一组，统一控制，电机一共就4种命令
	// 1.启动（读取返回数据）
	// 2.停止
	// 3.设置机器零位
	// 4.控制运行参数
	switch (leg_cmd.flags[0])
	{
	case Flag_Motor_Error:
		//对错误的不做任何处理，默认使用上次数据
		break;
	case Flag_Motor_Start:
		PackCANCmd(Flag_Motor_Start, Leg_One_Abad);
		PackCANCmd(Flag_Motor_Start, Leg_One_Hip);
		PackCANCmd(Flag_Motor_Start, Leg_One_Knee);
		break;
	case Flag_Motor_Stop:
		PackCANCmd(Flag_Motor_Stop, Leg_One_Abad);
		PackCANCmd(Flag_Motor_Stop, Leg_One_Hip);
		PackCANCmd(Flag_Motor_Stop, Leg_One_Knee);
		break;
	case Flag_Motor_SetZero:
		PackCANCmd(Flag_Motor_SetZero, Leg_One_Abad);
		PackCANCmd(Flag_Motor_SetZero, Leg_One_Hip);
		PackCANCmd(Flag_Motor_SetZero, Leg_One_Knee);
		break;
	case Flag_Motor_Control:
		PackCAN(leg_cmd.q_des_abad[0], leg_cmd.qd_des_abad[0], leg_cmd.kp_abad[0], leg_cmd.kd_abad[0], leg_cmd.tau_abad_ff[0], Leg_One_Abad);
		PackCAN(leg_cmd.q_des_hip[0], leg_cmd.qd_des_hip[0], leg_cmd.kp_hip[0], leg_cmd.kd_hip[0], leg_cmd.tau_hip_ff[0], Leg_One_Hip);
		PackCAN(leg_cmd.q_des_knee[0], leg_cmd.qd_des_knee[0], leg_cmd.kp_knee[0], leg_cmd.kd_knee[0], leg_cmd.tau_knee_ff[0], Leg_One_Knee);
		break;
		case Flag_Motor_Mode_Position:
		PackCANCmd(Flag_Motor_Mode_Position, Leg_One_Abad);
		PackCANCmd(Flag_Motor_Mode_Position, Leg_One_Hip);
		PackCANCmd(Flag_Motor_Mode_Position, Leg_One_Knee);
		break;
			case Flag_Motor_Mode_Velocity:
		PackCANCmd(Flag_Motor_Mode_Velocity, Leg_One_Abad);
		PackCANCmd(Flag_Motor_Mode_Velocity, Leg_One_Hip);
		PackCANCmd(Flag_Motor_Mode_Velocity, Leg_One_Knee);
		break;
				case Flag_Motor_Mode_Torque:
		PackCANCmd(Flag_Motor_Mode_Torque, Leg_One_Abad);
		PackCANCmd(Flag_Motor_Mode_Torque, Leg_One_Hip);
		PackCANCmd(Flag_Motor_Mode_Torque, Leg_One_Knee);
		break;
	default:
		// 默认是电机停止
		PackCANCmd(Flag_Motor_Stop, Leg_One_Abad);
		PackCANCmd(Flag_Motor_Stop, Leg_One_Hip);
		PackCANCmd(Flag_Motor_Stop, Leg_One_Knee);
		break;
	}

	switch (leg_cmd.flags[1])
	{
	case Flag_Motor_Error:
		//对错误的不做任何处理，默认使用上次数据
		break;
	case Flag_Motor_Start:
		PackCANCmd(Flag_Motor_Start, Leg_Two_Abad);
		PackCANCmd(Flag_Motor_Start, Leg_Two_Hip);
		PackCANCmd(Flag_Motor_Start, Leg_Two_Knee);
		break;
	case Flag_Motor_Stop:
		PackCANCmd(Flag_Motor_Stop, Leg_Two_Abad);
		PackCANCmd(Flag_Motor_Stop, Leg_Two_Hip);
		PackCANCmd(Flag_Motor_Stop, Leg_Two_Knee);
		break;
	case Flag_Motor_SetZero:
		PackCANCmd(Flag_Motor_SetZero, Leg_Two_Abad);
		PackCANCmd(Flag_Motor_SetZero, Leg_Two_Hip);
		PackCANCmd(Flag_Motor_SetZero, Leg_Two_Knee);
		break;
	case Flag_Motor_Control:
		PackCAN(leg_cmd.q_des_abad[1], leg_cmd.qd_des_abad[1], leg_cmd.kp_abad[1], leg_cmd.kd_abad[1], leg_cmd.tau_abad_ff[1], Leg_Two_Abad);
		PackCAN(leg_cmd.q_des_hip[1], leg_cmd.qd_des_hip[1], leg_cmd.kp_hip[1], leg_cmd.kd_hip[1], leg_cmd.tau_hip_ff[1], Leg_Two_Hip);
		PackCAN(leg_cmd.q_des_knee[1], leg_cmd.qd_des_knee[1], leg_cmd.kp_knee[1], leg_cmd.kd_knee[1], leg_cmd.tau_knee_ff[1], Leg_Two_Knee);
		break;
			case Flag_Motor_Mode_Position:
		PackCANCmd(Flag_Motor_Mode_Position, Leg_One_Abad);
		PackCANCmd(Flag_Motor_Mode_Position, Leg_One_Hip);
		PackCANCmd(Flag_Motor_Mode_Position, Leg_One_Knee);
		break;
			case Flag_Motor_Mode_Velocity:
		PackCANCmd(Flag_Motor_Mode_Velocity, Leg_One_Abad);
		PackCANCmd(Flag_Motor_Mode_Velocity, Leg_One_Hip);
		PackCANCmd(Flag_Motor_Mode_Velocity, Leg_One_Knee);
		break;
				case Flag_Motor_Mode_Torque:
		PackCANCmd(Flag_Motor_Mode_Torque, Leg_One_Abad);
		PackCANCmd(Flag_Motor_Mode_Torque, Leg_One_Hip);
		PackCANCmd(Flag_Motor_Mode_Torque, Leg_One_Knee);
		break;
	default:
		// 默认是电机停止
		PackCANCmd(Flag_Motor_Stop, Leg_Two_Abad);
		PackCANCmd(Flag_Motor_Stop, Leg_Two_Hip);
		PackCANCmd(Flag_Motor_Stop, Leg_Two_Knee);
		break;
	}
}
void CAN_Send_All(void)
{
	CAN_Send(CAN1, 0x01, &g_CAN_Leg_Data[0]);
	CAN_Send(CAN2, 0x01, &g_CAN_Leg_Data[24]);
	CAN_Send(CAN1, 0x02, &g_CAN_Leg_Data[8]);
	CAN_Send(CAN2, 0x02, &g_CAN_Leg_Data[32]);
	CAN_Send(CAN1, 0x03, &g_CAN_Leg_Data[16]);
	CAN_Send(CAN2, 0x03, &g_CAN_Leg_Data[40]);
}

//把电池和电流的采样也放入leg_msg里面
void Pack_ADC()
{
	for (int i = 0; i < 10; i++)
	{
		adc_f += adc_ordinary_valuetab[i];
	}
	adc_f = adc_f * 3.3 / 40960.0;
	leg_msg.BatorCurrent[0] = adc_f;
	leg_msg.BatorCurrent[1] = adc_f;
	// printf("ADC: %0.2f V\r\n", adc_f);
	//  printf("RX: \r\n");
	//  for (int i = 0; i < 24; i++)
	//  {
	//  	// printf("%x \t", SPI_Slave_Buffer_Rx[i]);
	//  	printf("%x \t", CAN2_Rx_Data[i]);
	//  }
}
int main(void)
{
	// 240M
	system_clock_config();

	// IRQ priority
	nvic_priority_group_config(NVIC_PRIORITY_GROUP_4);
	// init board resources
	at32_board_init();
	while (1)
	{
		LED_ON();
		Pack_ADC();

		// 如果SPI的DMA接收完了数据，准备开始处理
		if (dma_flag_get(SPI_Slave_Rx_DMA_FLAG) != RESET)
		{
			// SPI->SPI_Rx->Leg_cmd->CAN_Tx->CAN
			//解析从VIM3来的SPI数据
			UnpackSPI();
			//打印leg_cmd的数据
//						printf("leg_cmd_q_des_abad:%f,%f\r\n", leg_cmd.q_des_abad[0], leg_cmd.q_des_abad[1]);
//						printf("leg_cmd_q_des_hip:%f,%f\r\n", leg_cmd.q_des_hip[0], leg_cmd.q_des_hip[1]);
//						printf("leg_cmd_q_des_knee:%f,%f\r\n", leg_cmd.q_des_knee[0], leg_cmd.q_des_knee[1]);

//						printf("leg_cmd_qd_des_abad:%f,%f\r\n", leg_cmd.qd_des_abad[0], leg_cmd.qd_des_abad[1]);
//						printf("leg_cmd_qd_des_hip:%f,%f\r\n", leg_cmd.qd_des_hip[0], leg_cmd.qd_des_hip[1]);
//						printf("leg_cmd_qd_des_knee:%f,%f\r\n", leg_cmd.qd_des_knee[0], leg_cmd.qd_des_knee[1]);

//						printf("leg_cmd_kp_abad:%f,%f\r\n", leg_cmd.kp_abad[0], leg_cmd.kp_abad[1]);
//						printf("leg_cmd_kp_hip:%f,%f\r\n", leg_cmd.kp_hip[0], leg_cmd.kp_hip[1]);
//						printf("leg_cmd_kp_knee:%f,%f\r\n", leg_cmd.kp_knee[0], leg_cmd.kp_knee[1]);

//						printf("leg_cmd_kd_abad:%f,%f\r\n", leg_cmd.kd_abad[0], leg_cmd.kd_abad[1]);
//						printf("leg_cmd_kd_hip:%f,%f\r\n", leg_cmd.kd_hip[0], leg_cmd.kd_hip[1]);
//						printf("leg_cmd_kd_knee:%f,%f\r\n", leg_cmd.kd_knee[0], leg_cmd.kd_knee[1]);

//						printf("leg_cmd_tau_abad_ff:%f,%f\r\n", leg_cmd.tau_abad_ff[0], leg_cmd.tau_abad_ff[1]);
//						printf("leg_cmd_tau_hip_ff:%f,%f\r\n", leg_cmd.tau_hip_ff[0], leg_cmd.tau_hip_ff[1]);
//						printf("leg_cmd_tau_knee_ff:%f,%f\r\n", leg_cmd.tau_knee_ff[0], leg_cmd.tau_knee_ff[1]);

//						printf("leg_cmd_flags:%d,%d\r\n", leg_cmd.flags[0], leg_cmd.flags[1]);
//						printf("leg_cmd_checksum:%d\r\n", leg_cmd.checksum);
//			printf("ADC: %0.2f V\r\n", adc_f);
			// 打包CAN数据格式
			PackCAN_ALL();
			// 发送所有的CAN数据
			CAN_Send_All();
			// 清DMA标志位,需要清理多个，即开启下一次接收数据数据
			dma_flag_clear(SPI_Slave_Rx_DMA_FLAG | DMA1_GL2_FLAG | DMA1_HDT2_FLAG | DMA1_DTERR2_FLAG);
			dma_flag_clear(SPI_Slave_Tx_DMA_FLAG | DMA1_GL3_FLAG | DMA1_HDT3_FLAG | DMA1_DTERR3_FLAG);
		}

		delay_us(10);

		// 只要接受到任意电机CAN数据就处理
		// CAN->CAN_Rx->Leg_msg->SPI_Tx->SPI
		if (Flag_CAN_Rx != Flag_CAN_Clear)
		{
			//************************CAN1************************//
			//接收到第一个电机的数据
			if ((Flag_CAN_Rx & Flag_CAN1_ID1) == Flag_CAN1_ID1)
			{
				UnpackCAN(CAN1_Rx_Data, Leg_One, abad);
				//printf("CAN_ID_1:%f, %f, %f\r\n", leg_msg.q_abad[0], leg_msg.qd_abad[0], leg_msg.tau_abad[0]);
				// 清除标志位
				Flag_CAN_Rx &= ~(0x1 << 0);
			}

			//接收到第二个电机的数据
			if ((Flag_CAN_Rx & Flag_CAN1_ID2) == Flag_CAN1_ID2)
			{
				UnpackCAN(CAN1_Rx_Data, Leg_One, hip);

				printf("CAN_ID_2:%f, %f, %f\r\n", leg_msg.q_hip[0], leg_msg.qd_hip[0], leg_msg.tau_hip[0]);

				// 清除标志位
				Flag_CAN_Rx &= ~(0x1 << 1);
			}

			//接收到第三个电机的数据
			if ((Flag_CAN_Rx & Flag_CAN1_ID3) == Flag_CAN1_ID3)
			{
				UnpackCAN(CAN1_Rx_Data, Leg_One, knee);

				printf("CAN_ID_3:%f, %f, %f\r\n", leg_msg.q_knee[0], leg_msg.qd_knee[0], leg_msg.tau_knee[0]);

				// 清除标志位
				Flag_CAN_Rx &= ~(0x1 << 2);
			}

			//************************CAN2************************//
			//接收到第一个电机的数据
			if ((Flag_CAN_Rx & Flag_CAN2_ID1) == Flag_CAN2_ID1)
			{
				UnpackCAN(CAN1_Rx_Data, Leg_Two, abad);

				printf("CAN2_ID_1:%f,%f,%f", leg_msg.q_abad[1], leg_msg.qd_abad[1], leg_msg.tau_abad[1]);

				// 清除标志位
				Flag_CAN_Rx &= ~(0x1 << 4);
			}

			//接收到第二个电机的数据
			if ((Flag_CAN_Rx & Flag_CAN2_ID2) == Flag_CAN2_ID2)
			{
				UnpackCAN(CAN1_Rx_Data, Leg_Two, hip);

				printf("CAN2_ID_2:%f,%f,%f", leg_msg.q_hip[1], leg_msg.qd_hip[1], leg_msg.tau_hip[1]);

				// 清除标志位
				Flag_CAN_Rx &= ~(0x1 << 5);
			}

			//接收到第三个电机的数据
			if ((Flag_CAN_Rx & Flag_CAN2_ID3) == Flag_CAN2_ID3)
			{
				UnpackCAN(CAN1_Rx_Data, Leg_Two, knee);

				printf("CAN2_ID_3:%f,%f,%f", leg_msg.q_knee[1], leg_msg.qd_knee[1], leg_msg.tau_knee[1]);

				// 清除标志位
				Flag_CAN_Rx &= ~(0x1 << 6);
			}
			PackSPI();
		}
	}
}
